#include "liolib/lioflat.h"
#include "liolib/liocom.h"

#define DEFAULT_IP _T("192.168.0.103")
#define DEFAULT_PORT _T("15000")
#define NORM_VOLT(V) (float(V)/255)
#define SECT_TO_RPM(sect) (0.8f*(sect))
#define NORM_IR(ir) INV(150-ir)
#define MIN_IR 20
#define MAX_IR 150
#define MODE_FREE 0
#define MODE_CTRL 128
#define DIR_FORWARD 64
#define DIR_BACKWARD 0
#define MAX_SPEED_FREE 60
#define MAX_SPEED_CTRL 40
#define SRC_FILE_IN_NAME _T("data_in.dat")
#define SRC_FILE_OUT_NAME _T("data_out.dat")
#define RCV_THREAD_SLEEP 0
#define SEND_THREAD_SLEEP 60
#define GRAPH_POS 300
#define DEF_BUFFER_SIZE 100
#define DEF_FIR_ORDER 31
#define DEF_CUTOFF_FREQ 5
#define MIN_DIST 0.1
#define MAX_DIST 1.7

namespace Lio {

enum IR {
	IR_LEFT,
	IR_RIGHT,
	IR_COUNT};

const float irCoefs[IR_COUNT][2]={
	{+0.057734f,+28.195151f},
	{-0.062668f,+36.057799f}};

inline float getDist(float ir, IR id)
{
	if(ir<MIN_IR)
		ir=MIN_IR;
	else if(ir>MAX_IR)
		ir=MAX_IR;
	return irCoefs[id][0]+irCoefs[id][1]/ir;
}

enum Sources {
	SOURCE_WIFIBOT,
	SOURCE_FILE,
	SOURCE_COUNT};

// coefR={3.742,-14.262,23.461,-17.217,4.615}
// coefL={2.830,-9.421,14.354,-9.961,2.542}

// Sensor Struct:
struct SensorData
{
	float	BatVoltage, // 0-1
			SpeedFrontLeft, // rpm
			SpeedRearLeft, // rpm
			SpeedFrontRight, // rpm
			SpeedRearRight, // rpm
			IRLeft, // 0-1
			IRRight, // 0-1
			DistLeft, // m
			DistRight, // m
			Timer; // s
};
struct WifibotReceive
{
	BYTE	data[7];
	float	period;
};

// Wifibot Communication Class:
class Wifibotsc12: public Runnable
{
protected:
	CriticalSection cs,cs_connect,cs_receive,cs_send;
	Socket socket;
	volatile BOOL threadRun;
	BYTE sendbuf[2],rcvbuf[7];
	List<WifibotReceive> rcvList;
	PrecisionTimer timer;
	ThreadInfo send, receive;
public:
	BOOL control;
	Wifibotsc12();
	~Wifibotsc12();
	
	// Send Control Routines
	BOOL SetComControl(BYTE coml, BYTE comr);
	
	// Control Convertions:
	void SpeedToWheelControl(double Vrot, double Vlin, double * left, double * right);
	void WheelToSpeedControl(double left, double right, double * Vrot, double * Vlin);
	double WheelToComControl(double left, double right, BYTE * coml, BYTE * comr);
	void JoyToComControl(int x, int y, BYTE * coml, BYTE * comr);

	BOOL GetNextData(WifibotReceive *rcvbuf);
	static void ConvertData(const WifibotReceive*, SensorData*);
	BOOL Connect(const char * ip,int port);
	void Disconnect();
	int getWaitingCount() const {return rcvList.length;}
	DWORD run(const ThreadInfo *info);
};
// WifibotApplication - Class For The Application:
// FlatApplication - base class, provides all necessary windows routines
class WifibotApplication:public FlatApplication
{
protected:
	// ip - String containing the IP address
	// port - Port for communication
	String ip;
	int port;

	ThreadInfo operate, connect;

	// Checking keyboard:
	BOOL space;

	// Font
	Font fontSmall,fontBold;

	// Wifibot commands:
	BYTE coml,comr;

	// Struct for formating the incoming data from Wifibot
	SensorData robotSensors;

	// Source file
	File srcFileOut,srcFileIn;

	// Receive buffer:
	WifibotReceive rcvbuf;

	enum ChnlsIRid{
		CHNL_TIME,
		CHNL_IRL,
		CHNL_IRR,
		CHNL_FIRL,
		CHNL_FIRR,
		CHNL_IR_COUNT
	};
	double time,T,fs,skirtLength,cutOffFreq;
	Channel *chnlsIR[CHNL_IR_COUNT];
	int buffSize,coefCount,countL,countR,filterOrder,windowType;
	Vector<double>	buffIRL,buffIRR,buffTime,buffFIRL,buffFIRR,filter;
	BOOL filterOK;

	// Window controls: Labels, TextBoxes, Scrolls and Buttons for the Main Window
	FlatGraph fgIR;
	FlatLabel lSource,lOrder,lWindow,lCutOff;
	FlatChoice chSource,chWindow;
	FlatText tbIP,tbPort,tbMem,tbOrder,tbCutOff;
	FlatButton bConnect,bSave,bExit,bMem,bFilter;

	// robot - The object containg all necessary routines for communication with Wifibot
	Wifibotsc12 robot;
	

	// Application specific functions
	// create - Invoked when the application starts for window and user initializations
	// glCreate - Invoked for OpenGl initialization
	// glDestroy - Invoked for OpenGl termination
	// destroy - Invoked when terminating the application
	// reshape - Invoked when resizing the window
	// listen - Invoked when receiving a message
	// update - Main program loop body, place update code here
	// draw2D - Procedure for drawing in 2D
	// draw3D - Procedure for drawing in 3D
	BOOL create();
	BOOL glCreate();
	void glDestroy();
	void reshape ();
	BOOL listen(WORD id, WORD code, const void *info);
	DWORD run(const ThreadInfo *info);
	// action when data received from Wifibot
	virtual void receiveAction(){};
	// action when connected
	virtual void updateAction(){}
	// action when connected or disconnected
	virtual void connectAction(){}
	// action when created
	virtual BOOL createAction(){return TRUE;}
	// action when listening
	virtual BOOL listenAction(WORD id, WORD code, const void *info){return FALSE;}
	// action when reshaping
	virtual void reshapeAction(){}
	// Buffers shift
	void shift();
	// changing the buffer size
	virtual BOOL setBufferSize(int size);
	// Filter parameters
	void setFilter(int order, int window, double cutOffFreq);
public:
	// Constructor Initialization:
	// 0.3 - wheel distance
	// 1 - max speed
	WifibotApplication();
};

} // namespace Lio